/*
 * @Descripttion: 
 * @version: V1.0.0
 * @Author: LeonHe
 * @Date: 2021-05-29 20:10:00
 * @LastEditTime: 2021-05-29 20:18:37
 */
#include "commonfilter.h"

q15_t avgFilterQ15(avgFilterQ15StDef *inst, q15_t in)
{
    q15_t out;

    inst->sum -= inst->buff[inst->index];
    inst->sum += in;
    inst->buff[inst->index] = in;

    out = inst->sum/inst->deepth;
    
    if(++inst->index >= inst->deepth)
        inst->index = 0;

    return out;
}

q15_t avgLimitFilterQ15(avgLimitFilterQ15StDef *inst, q15_t in)
{
    q15_t out;

    //限幅：超过幅度则直接使用上个周期的值
    if(inst->buff[inst->index] - in > inst->Amplitude ||
        in - inst->buff[inst->index] > inst->Amplitude)
    {
        return inst->sum/inst->deepth;
    }
    
    if(++inst->index >= inst->deepth)
        inst->index = 0;

    inst->sum -= inst->buff[inst->index];
    inst->sum += in;
    inst->buff[inst->index] = in;

    out = inst->sum/inst->deepth;

    return out;
}

q31_t avgLimitFilterQ31(avgLimitFilterQ31StDef *inst, q31_t in)
{
    q31_t out;

    //限幅：超过幅度则直接使用上个周期的值
    if(inst->buff[inst->index] - in > inst->Amplitude ||
        in - inst->buff[inst->index] > inst->Amplitude)
    {
        return inst->sum/inst->deepth;
    }
    
    if(++inst->index >= inst->deepth)
        inst->index = 0;

    inst->sum -= inst->buff[inst->index];
    inst->sum += in;
    inst->buff[inst->index] = in;

    out = inst->sum/inst->deepth;

    return out;
}
